#include<stdio.h>
#include <stdint.h>
#include<iostream>
#include<vector>
#include "include/ClutterMap.h"
#include <math.h>

using namespace std;


ClutterMap::ClutterMap(int rSize, int aziSize, int pitchSize, int doppSize)
        : r_size(rSize), azi_size(aziSize), pitch_size(pitchSize), dopp_size(doppSize) {
    // 初始化四维数组
//    clutter_map.resize(r_size, std::vector<std::vector<std::vector<int>>>(azi_size,
//                                                                          std::vector<std::vector<int>>(pitch_size,
//                                                                                                        std::vector<int>(
//                                                                                                                dopp_size,
//                                                                                                                0))));
    vector<PointIn> clutter_map;
}

//
void ClutterMap::isClutterMap(std::vector<PointIn> &input, std::vector<PointIn> &output) {
    float range_th = 1;
    float azimuth_th = 1;
    float pitch_th = 1;
    float doppler_th = 0.5;
    bool flag;
    for (const PointIn &point: input) {
        flag = true;
        for (const PointIn &clutter: clutter_map) {
            float range_change = abs(point.Range - clutter.Range);
            float azimuth_change = abs(point.Azimuth - clutter.Azimuth);
            float pitch_change = abs(point.pitch - clutter.pitch);
            float doppler_change = abs(point.Doppler - clutter.Doppler);
            if (range_change <= range_th && azimuth_change <= azimuth_th &&
                doppler_change <= doppler_th) {
                //如果是杂波，直接返回
                flag = false;
                break;
            }
        }
        if (flag) output.push_back(point);
    }

    //更新杂波图
    clutter_map.clear();
    clutter_map.insert(clutter_map.begin(), input.begin(), input.end());
#if 0
    // 遍历输入点迹
//    for (const PointIn &point: input) {
//        // 将物理量映射为索引
//        int rIdx = point.Range / 3;
//        int aziIdx = (point.Azimuth + 25);
//        int pitchIdx = (point.pitch +25);
//        int doppIdx = point.Doppler;
//
//        //判断是否为目标
//        if(clutter_map[rIdx][aziIdx][pitchIdx][doppIdx]<10) {
//            output.push_back(point);
//        }
//        clutter_map[rIdx][aziIdx][pitchIdx][doppIdx] += point.SNR;
//    }
    //更新杂波图
//    for (int i = 0; i < r_size; ++i) {
//        for (int j = 0; j < azi_size; ++j) {
//            for (int k = 0; k < pitch_size; ++k) {
//                for (int l = 0; l < dopp_size; ++l) {
//                    clutter_map[i][j][k][l] *=0.5;
//                }
//            }
//        }
//    }
#endif
}